#include <QCoreApplication>
#include <sched.h>
#include<stdio.h>
#include <sys/mman.h>

#include "chainSolverTest.hpp"
#include "pathTest.hpp"

#define NSEC_PER_SEC    1000000000 /* The number of nsecs per sec. */
#define MY_PRIORITY    99

using namespace KDL;

void upPriority(int priority)
{
    struct sched_param param;
    param.__sched_priority = priority;
    if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
    {
        perror("sched_setscheduler failed");
    }

    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
    {
        perror("mlockall failed");
    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    upPriority(99);
    struct sched_param param;
    param.__sched_priority = 99;
    sched_setscheduler(0,SCHED_FIFO,&param);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
    ChainSolverTest* solverTest = new ChainSolverTest();
    solverTest->chainFkPosSolverTest();
 //   solverTest->chainIkPosSolverTest_LMA();
//    solverTest->chainIkPosSolverTest_NR();
//    solverTest->chainIkPosSolverTest_NR_JL();
//    solverTest->chainFkandIkVelTest();
//    solverTest->chainIkVelTest_nso_wdls();
////    solverTest->chainFkandIkAccTest();
//    solverTest->chainIdSolverTest();
////    delete solverTest;
////    solverTest=NULL;
//    PathTest* pathTest = new PathTest();
//    pathTest->pathLineTest();
//    pathTest->pathPointTest();
//    pathTest->pathCircleTest();
//    delete pathTest;
//    pathTest=NULL;
    return a.exec();
}
